/*
 * @Author       : jiejie
 * @GitHub       : https://github.com/jiejieTop
 * @Date         : 2022-03-16 13:56:02
 * @LastEditors  : jiejie
 * @LastEditTime : 2022-12-01 10:52:29
 * @FilePath     : /middleware_benchmark/ecal/loc/loc_rec_snd/src/loc_rec_snd.cpp
 * Copyright (c) 2022 jiejie, All Rights Reserved. Please keep the author
 * information and source code according to the license.
 */

#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

#include <sys/stat.h>
#include <sys/time.h>
#include <tclap/CmdLine.h>
#include <unistd.h>

#include <algorithm>
#include <chrono>
#include <iostream>
#include <numeric>
#include <queue>
#include <thread>

#include "loc.pb.h"

bool send_flag = false;

bool loc_stop = false;

int sleep_time(0);
// warmup runs not to measure
const int warmups(5);

// 传感器数据同步时间为100ms
const int sync_time(100);

const int evaluate_count(20000);

std::stringstream sync_all_info;
std::stringstream all_info;

std::string loc_stop_sub_topic{ "loc_stop_signal" };
std::string loc_stop_pub_topic{ "loc_stop_signal" };

std::vector<std::string> loc_sub_topics{ "/ecal/gps/fix",           "/ecal/gps/odom",
                                         "/ecal/imu/data",          "/ecal/localization/fix",
                                         "/ecal/localization/odom", "/ecal/location/outdoor/odom" };
std::vector<std::string> loc_pub_topics;

std::map<std::string, eCAL::protobuf::CPublisher<pb::Imu>*> imu_publishers;
std::map<std::string, eCAL::protobuf::CPublisher<pb::NavSatFix>*> fix_publishers;
std::map<std::string, eCAL::protobuf::CPublisher<pb::Odometry>*> odom_publishers;

std::mutex fix_mutex;
std::mutex imu_mutex;
std::mutex odom_mutex;

// data structure for later evaluation
struct evaluate_data {
    evaluate_data()
    {
        seq_array.reserve(4000000);
        comm_latency_array.reserve(4000000);
        ecal_to_ros_latency_array.reserve(4000000);
        ros_to_ecal_latency_array.reserve(4000000);
    };
    uint32_t send_count;
    std::vector<uint32_t> seq_array;
    std::vector<float> comm_latency_array;
    std::vector<float> ecal_to_ros_latency_array;
    std::vector<float> ros_to_ecal_latency_array;
    size_t rec_size = 0;
    size_t msg_num = 0;
};

std::map<std::string, evaluate_data*> evaluate_map;

void evaluate(evaluate_data* data_, const std::string file_name_);

void OnNavSatFix(const char* topic_name_, const pb::NavSatFix& fix_, const long long time_, const long long clock_)
{
    static int recv_count = 0;
    // get receive time stamp
    auto rec_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();
    evaluate_data* data = nullptr;
    auto it = evaluate_map.find(topic_name_);
    if (it == evaluate_map.end()) {
        data = new evaluate_data();
        evaluate_map[topic_name_] = data;
        data->send_count = fix_.send_count();
        std::cout << "receive fix by topic " << topic_name_ << ", send count [" << fix_.send_count() << "]"
                  << std::endl;
    } else {
        data = it->second;
    }

    auto start_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();
    static thread_local sensor_msgs::NavSatFix ros_fix;

    ros_fix.header.seq = fix_.header().seq();
    ros_fix.header.stamp.sec = fix_.header().timestamp() / 1000000;
    ros_fix.header.stamp.nsec = fix_.header().timestamp() * 1000;
    ros_fix.header.frame_id = fix_.header().frame_id();

    ros_fix.status.status = fix_.status().status();
    ros_fix.status.service = fix_.status().service();

    ros_fix.latitude = fix_.latitude();
    ros_fix.longitude = fix_.longitude();
    ros_fix.altitude = fix_.altitude();

    for (int i = 0; i < fix_.position_covariance_size(); i++) {
        ros_fix.position_covariance[i] = fix_.position_covariance(i);
    }

    ros_fix.position_covariance_type = fix_.position_covariance_type();

    auto end_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();

    // update latency, size and msg number
    data->comm_latency_array.push_back((rec_time - time_) / 1000.0);
    data->rec_size += fix_.ByteSize();
    data->msg_num++;

    // 计算时间戳
    data->ecal_to_ros_latency_array.push_back((end_time - start_time) / 1000.0);
    data->ros_to_ecal_latency_array.push_back(fix_.convert_time());

    if (data->send_count < fix_.send_count()) {
        data->send_count = fix_.send_count();
    }

    while ((data->seq_array.size() < fix_.header().seq() - 1) && (data->seq_array.size() < data->send_count)) {
        data->seq_array.push_back(0);
    }
    if ((data->seq_array.size() >= fix_.header().seq() - 1) && (data->seq_array.size() < data->send_count)) {
        data->seq_array[fix_.header().seq() - 1] = fix_.header().seq();
    }
    data->seq_array.push_back(fix_.header().seq());

    // 发布 ecal topic
    if (send_flag) {
        auto pub_it = fix_publishers.find(topic_name_);
        if (pub_it != fix_publishers.end()) {
            auto pub_time = std::chrono::duration_cast<std::chrono::microseconds>(
                                std::chrono::system_clock::now().time_since_epoch())
                                .count();
            pub_it->second->Send(fix_, pub_time);
        }
    }

    if (fix_.header().seq() % evaluate_count == 0) {
        std::cout << "recv count: " << fix_.header().seq() << std::endl;
        data->send_count = fix_.header().seq();
        evaluate(data, std::string(topic_name_));
        data->send_count = fix_.send_count();
    }

    // ros::ros_helper::instance().publish("/ecal/" + std::string(topic_name_), ros_fix);
    if (sleep_time > 0) {
        if (fix_.header().seq() % evaluate_count == 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds((sleep_time - 20) > 0 ? (sleep_time - 20) : 1));
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
        }
    }
}

void OnImu(const char* topic_name_, const pb::Imu& imu_, const long long time_, const long long clock_)
{
    // get receive time stamp
    auto rec_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();
    evaluate_data* data = nullptr;
    auto it = evaluate_map.find(topic_name_);
    if (it == evaluate_map.end()) {
        data = new evaluate_data();
        evaluate_map[topic_name_] = data;
        data->send_count = imu_.send_count();
        std::cout << "receive point cloud by topic " << topic_name_ << ", send count [" << imu_.send_count() << "]"
                  << std::endl;
    } else {
        data = it->second;
    }

    auto start_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();
    static thread_local sensor_msgs::Imu ros_imu;

    ros_imu.header.seq = imu_.header().seq();
    ros_imu.header.stamp.sec = imu_.header().timestamp() / 1000000;
    ros_imu.header.stamp.nsec = imu_.header().timestamp() * 1000;
    ros_imu.header.frame_id = imu_.header().frame_id();

    ros_imu.orientation.x = imu_.orientation().x();
    ros_imu.orientation.y = imu_.orientation().y();
    ros_imu.orientation.z = imu_.orientation().z();
    ros_imu.orientation.w = imu_.orientation().w();

    for (int i = 0; i < imu_.orientation_covariance_size(); i++) {
        ros_imu.orientation_covariance[i] = imu_.orientation_covariance(i);
    }

    ros_imu.angular_velocity.x = imu_.angular_velocity().x();
    ros_imu.angular_velocity.y = imu_.angular_velocity().y();
    ros_imu.angular_velocity.z = imu_.angular_velocity().z();

    for (int i = 0; i < imu_.angular_velocity_covariance_size(); i++) {
        ros_imu.angular_velocity_covariance[i] = imu_.angular_velocity_covariance(i);
    }

    ros_imu.linear_acceleration.x = imu_.linear_acceleration().x();
    ros_imu.linear_acceleration.y = imu_.linear_acceleration().y();
    ros_imu.linear_acceleration.z = imu_.linear_acceleration().z();

    for (int i = 0; i < imu_.linear_acceleration_covariance_size(); i++) {
        ros_imu.linear_acceleration_covariance[i] = imu_.linear_acceleration_covariance(i);
    }

    auto end_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();

    // update latency, size and msg number
    data->comm_latency_array.push_back((rec_time - time_) / 1000.0);
    data->rec_size += imu_.ByteSize();
    data->msg_num++;

    // 计算时间戳
    data->ecal_to_ros_latency_array.push_back((end_time - start_time) / 1000.0);
    data->ros_to_ecal_latency_array.push_back(imu_.convert_time());

    if (data->send_count < imu_.send_count()) {
        data->send_count = imu_.send_count();
    }

    while ((data->seq_array.size() < imu_.header().seq() - 1) && (data->seq_array.size() < data->send_count)) {
        data->seq_array.push_back(0);
    }
    if ((data->seq_array.size() >= imu_.header().seq() - 1) && (data->seq_array.size() < data->send_count)) {
        data->seq_array[imu_.header().seq() - 1] = imu_.header().seq();
    }
    data->seq_array.push_back(imu_.header().seq());

    // ros::ros_helper::instance().publish("/ecal/" + std::string(topic_name_), ros_imu);

    // 发布 ecal topic
    if (send_flag) {
        auto pub_it = imu_publishers.find(topic_name_);
        if (pub_it != imu_publishers.end()) {
            auto pub_time = std::chrono::duration_cast<std::chrono::microseconds>(
                                std::chrono::system_clock::now().time_since_epoch())
                                .count();
            pub_it->second->Send(imu_, pub_time);
        }
    }

    if (imu_.header().seq() % evaluate_count == 0) {
        std::cout << "recv count: " << imu_.header().seq() << std::endl;
        data->send_count = imu_.header().seq();
        evaluate(data, std::string(topic_name_));
        data->send_count = imu_.send_count();
    }

    if (sleep_time > 0) {
        if (imu_.header().seq() % evaluate_count == 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds((sleep_time - 20) > 0 ? (sleep_time - 20) : 1));
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
        }
    }
}

void OnOdometry(const char* topic_name_, const pb::Odometry& odom_, const long long time_, const long long clock_)
{
    auto rec_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();

    evaluate_data* data = nullptr;
    auto it = evaluate_map.find(topic_name_);
    if (it == evaluate_map.end()) {
        data = new evaluate_data();
        evaluate_map[topic_name_] = data;
        data->send_count = odom_.send_count();
        std::cout << "receive point cloud by topic " << topic_name_ << ", send count [" << odom_.send_count() << "]"
                  << std::endl;
    } else {
        data = it->second;
    }

    auto start_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();

    static thread_local nav_msgs::Odometry ros_odom;

    ros_odom.header.seq = odom_.header().seq();
    ros_odom.header.stamp.sec = odom_.header().timestamp() / 1000000;
    ros_odom.header.stamp.nsec = odom_.header().timestamp() * 1000;
    ros_odom.header.frame_id = odom_.header().frame_id();

    ros_odom.child_frame_id = odom_.child_frame_id();

    ros_odom.pose.pose.position.x = odom_.pose().pose().position().x();
    ros_odom.pose.pose.position.y = odom_.pose().pose().position().y();
    ros_odom.pose.pose.position.z = odom_.pose().pose().position().z();

    ros_odom.pose.pose.orientation.x = odom_.pose().pose().orientation().x();
    ros_odom.pose.pose.orientation.y = odom_.pose().pose().orientation().y();
    ros_odom.pose.pose.orientation.z = odom_.pose().pose().orientation().z();
    ros_odom.pose.pose.orientation.w = odom_.pose().pose().orientation().w();

    for (int i = 0; i < odom_.pose().covariance_size(); i++) {
        // ros_odom.pose.covariance.assign()
    }

    ros_odom.twist.twist.linear.x = odom_.twist().twist().linear().x();
    ros_odom.twist.twist.linear.y = odom_.twist().twist().linear().y();
    ros_odom.twist.twist.linear.z = odom_.twist().twist().linear().z();

    ros_odom.twist.twist.angular.x = odom_.twist().twist().angular().x();
    ros_odom.twist.twist.angular.y = odom_.twist().twist().angular().y();
    ros_odom.twist.twist.angular.z = odom_.twist().twist().angular().z();

    for (int i = 0; i < odom_.twist().covariance_size(); i++) {
        ros_odom.twist.covariance[i] = odom_.twist().covariance(i);
    }

    auto end_time =
        std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch())
            .count();

    data->comm_latency_array.push_back((rec_time - time_) / 1000.0);
    data->rec_size += odom_.ByteSize();
    data->msg_num++;

    // 计算时间戳
    data->ecal_to_ros_latency_array.push_back((end_time - start_time) / 1000.0);
    data->ros_to_ecal_latency_array.push_back(odom_.convert_time());

    if (data->send_count < odom_.send_count()) {
        data->send_count = odom_.send_count();
    }

    while ((data->seq_array.size() < odom_.header().seq() - 1) && (data->seq_array.size() < data->send_count)) {
        data->seq_array.push_back(0);
    }
    if ((data->seq_array.size() >= odom_.header().seq() - 1) && (data->seq_array.size() < data->send_count)) {
        data->seq_array[odom_.header().seq() - 1] = odom_.header().seq();
    }
    data->seq_array.push_back(odom_.header().seq());

    // ros::ros_helper::instance().publish("/ecal/" + std::string(topic_name_), ros_imu);

    // 发布 ecal topic
    if (send_flag) {
        auto pub_it = odom_publishers.find(topic_name_);
        if (pub_it != odom_publishers.end()) {
            auto pub_time = std::chrono::duration_cast<std::chrono::microseconds>(
                                std::chrono::system_clock::now().time_since_epoch())
                                .count();
            pub_it->second->Send(odom_, pub_time);
        }
    }

    if (odom_.header().seq() % evaluate_count == 0) {
        std::cout << "recv count: " << odom_.header().seq() << std::endl;
        data->send_count = odom_.header().seq();
        evaluate(data, std::string(topic_name_));
        data->send_count = odom_.send_count();
    }

    if (sleep_time > 0) {
        if (odom_.header().seq() % evaluate_count == 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds((sleep_time - 20) > 0 ? (sleep_time - 20) : 1));
        } else {
            std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
        }
    }
}

bool mkdirs(std::string path)
{
    std::string p = "";
    std::string _path = "";
    std::istringstream ss;

    if (path.at(0) == '~') {
        std::string home = std::string(getenv("HOME"));
        ss.str(home + path.substr(1, path.size() - 1));
    } else {
        ss.str(path);
    }

    while (std::getline(ss, p, '/')) {
        _path += p + "/";
        if (access(_path.c_str(), F_OK) != 0) {
            if (::mkdir(_path.c_str(), 0777) == -1) {
                return false;
            }
        }
    }

    return true;
}

// evaluation
void evaluate(evaluate_data* data_, const std::string file_name_)
{
    uint32_t loss = 0;
    uint32_t seq_start = 0;
    std::stringstream total_ss;
    std::stringstream ss;
    float total_latency = 0.0;

    std::cout << "-------------------- " << file_name_ << " ----------------------" << std::endl;

    // remove warmup runs
    if (data_->comm_latency_array.size() >= warmups) {
        data_->comm_latency_array.erase(data_->comm_latency_array.begin(), data_->comm_latency_array.begin() + warmups);
    }

    if (data_->ecal_to_ros_latency_array.size() >= warmups) {
        data_->ecal_to_ros_latency_array.erase(data_->ecal_to_ros_latency_array.begin(),
                                               data_->ecal_to_ros_latency_array.begin() + warmups);
    }

    if (data_->ros_to_ecal_latency_array.size() >= warmups) {
        data_->ros_to_ecal_latency_array.erase(data_->ros_to_ecal_latency_array.begin(),
                                               data_->ros_to_ecal_latency_array.begin() + warmups);
    }

    for (int i = 0; i < data_->send_count; i++) {
        if (data_->seq_array[i] != 0) {
            if (seq_start == 0) {
                seq_start = i;
                break;
            }
        }
    }

    std::cout << "seq start : " << seq_start << std::endl;

    // std::cout << "loss data seq: ";
    for (int i = 0; i < data_->send_count; i++) {
        if (data_->seq_array[i] == 0) {
            // std::cout << i + 1 << " ";
            loss++;
        }
    }

    // std::cout << std::endl;

    auto loss_rate = (((loss - seq_start) * 1.0) / ((data_->send_count - seq_start) * 1.0)) * 100;

    std::cout << "loss message count : " << (((loss - seq_start) < 0 ? 0 : (loss - seq_start))) << std::endl;
    std::cout << "loss rate : " << loss_rate << " %" << std::endl;

    // evaluate all
    size_t sum_msg = data_->comm_latency_array.size();

    ss << "Messages received                            : " << sum_msg << std::endl;

    // evaluate ros to ecal
    sum_msg = data_->ros_to_ecal_latency_array.size();
    auto sum_time =
        std::accumulate(data_->ros_to_ecal_latency_array.begin(), data_->ros_to_ecal_latency_array.end(), 0.0f);
    auto avg_time = sum_time / sum_msg;
    auto min_it = std::min_element(data_->ros_to_ecal_latency_array.begin(), data_->ros_to_ecal_latency_array.end());
    auto max_it = std::max_element(data_->ros_to_ecal_latency_array.begin(), data_->ros_to_ecal_latency_array.end());
    auto min_pos = min_it - data_->ros_to_ecal_latency_array.begin();
    auto max_pos = max_it - data_->ros_to_ecal_latency_array.begin();
    auto min_time = *min_it;
    auto max_time = *max_it;
    total_latency += avg_time;

    ss << "ROS to eCAL PointCloud average latency       : " << avg_time << " ms" << std::endl;
    ss << "ROS to eCAL PointCloud min latency           : " << min_time << " ms @ " << min_pos << std::endl;
    ss << "ROS to eCAL PointCloud max latency           : " << max_time << " ms @ " << max_pos << std::endl;

    total_ss << data_->send_count << " " << sum_msg + warmups << " " << seq_start << " ";

    total_ss << avg_time << " " << min_time << " " << max_time << " ";

    sum_msg = data_->comm_latency_array.size();
    sum_time = std::accumulate(data_->comm_latency_array.begin(), data_->comm_latency_array.end(), 0.0f);
    avg_time = sum_time / (sum_msg * 1.0);
    min_it = std::min_element(data_->comm_latency_array.begin(), data_->comm_latency_array.end());
    max_it = std::max_element(data_->comm_latency_array.begin(), data_->comm_latency_array.end());
    min_pos = min_it - data_->comm_latency_array.begin();
    max_pos = max_it - data_->comm_latency_array.begin();
    min_time = *min_it;
    max_time = *max_it;
    total_latency += avg_time;

    auto size_per_msg = data_->rec_size / ((sum_msg * 1024.0) * 1.0);
    auto kb_per_sec = static_cast<int>(((data_->rec_size) / 1024.0) / (sum_time / 1000.0));
    auto mb_per_sec = static_cast<int>(((data_->rec_size) / 1024.0 / 1024.0) / (sum_time / 1000.0));
    auto msg_per_sec = static_cast<int>((sum_msg * 1.0) / (sum_time / 1000.0));

    ss << "Message average latency                      : " << avg_time << " ms" << std::endl;
    ss << "Message min latency                          : " << min_time << " ms @ " << min_pos << std::endl;
    ss << "Message max latency                          : " << max_time << " ms @ " << max_pos << std::endl;

    total_ss << avg_time << " " << min_time << " " << max_time << " ";

    // evaluate ecal to ros
    sum_msg = data_->ecal_to_ros_latency_array.size();
    sum_time = std::accumulate(data_->ecal_to_ros_latency_array.begin(), data_->ecal_to_ros_latency_array.end(), 0.0f);
    avg_time = sum_time / sum_msg;
    min_it = std::min_element(data_->ecal_to_ros_latency_array.begin(), data_->ecal_to_ros_latency_array.end());
    max_it = std::max_element(data_->ecal_to_ros_latency_array.begin(), data_->ecal_to_ros_latency_array.end());
    min_pos = min_it - data_->ecal_to_ros_latency_array.begin();
    max_pos = max_it - data_->ecal_to_ros_latency_array.begin();
    min_time = *min_it;
    max_time = *max_it;
    total_latency += avg_time;

    ss << "eCAL to ROS PointCloud average latency       : " << avg_time << " ms" << std::endl;
    ss << "eCAL to ROS PointCloud min latency           : " << min_time << " ms @ " << min_pos << std::endl;
    ss << "eCAL to ROS PointCloud max latency           : " << max_time << " ms @ " << max_pos << std::endl;

    ss << "Message size received                        : " << data_->rec_size / 1024 << " kB" << std::endl;
    ss << "Size of each message                         : " << size_per_msg << " kB" << std::endl;
    ss << "Throughput                                   : " << kb_per_sec << " kB/s" << std::endl;
    ss << "                                             : " << mb_per_sec << " MB/s" << std::endl;
    ss << "                                             : " << msg_per_sec << " Msg/s" << std::endl;

    total_ss << avg_time << " " << min_time << " " << max_time << " ";

    total_ss << total_latency << " " << mb_per_sec << " " << msg_per_sec << " " << loss_rate << std::endl;

    ss << "--------------------------------------------" << std::endl;

    // log to console
    std::cout << ss.str();
    std::cout << total_ss.str();

    all_info << file_name_ << " " << total_ss.str();

    // write to file

    timeval tv;
    ::gettimeofday(&tv, nullptr);
    time_t t = (tv.tv_sec * 1000000 + tv.tv_usec) / 1000000;
    tm* tm = localtime(&t);
    char str[100];
    int slen = strftime(str, 100, "%Y-%m-%d-%H:%M:%S", tm);
    auto time = std::string(str, slen);

    // mkdirs("./evaluate/");

    // FILE* csv_file = fopen(("evaluate/" + file_name_ + "-" + time + ".csv").c_str(), "w");
    // std::string info("序号, ros转ecal耗时(ms), 传输耗时(ms), ecal转ros耗时(ms), 总耗时(ms)\n");
    // fwrite(info.c_str(), 1, info.size(), csv_file);

    // for (int i = 0; i < data_->comm_latency_array.size(); i++) {
    //     std::stringstream data_ss;
    //     auto total =
    //         data_->ros_to_ecal_latency_array[i] + data_->comm_latency_array[i] + data_->ecal_to_ros_latency_array[i];
    //     data_ss << i << ", " << data_->ros_to_ecal_latency_array[i] << ", " << data_->comm_latency_array[i] << ", "
    //             << data_->ecal_to_ros_latency_array[i] << ", " << total << std::endl;
    //     fwrite(data_ss.str().c_str(), 1, data_ss.str().size(), csv_file);
    // }

    // fclose(csv_file);
}

int main(int argc, char** argv)
{
    std::thread sync_thread;
    // initialize eCAL API
    eCAL::Initialize(argc, argv, "loc_subscriber");

    TCLAP::CmdLine cmd("loc_subscriber");

    // set process state
    eCAL::Process::SetState(proc_sev_healthy, proc_sev_level1, "I feel good !");

    TCLAP::ValueArg<int> topic_num("t", "topic", "number of recv topic number", false, 0, "int");
    TCLAP::ValueArg<int> sleep_delay("d", "delay", "delay on OnImu imu_callback.", false, 0, "int");
    TCLAP::ValueArg<int> node_number("n", "node_number", "node number", false, 1, "int");
    TCLAP::SwitchArg no_send_flag("", "no-send", "no send flag", false);

    cmd.add(topic_num);
    cmd.add(sleep_delay);
    cmd.add(node_number);
    cmd.add(no_send_flag);
    cmd.parse(argc, argv);

    send_flag = !no_send_flag.getValue();
    sleep_time = sleep_delay.getValue();

    std::vector<eCAL::CSubscriber> stop_signal_subscribers;
    std::vector<eCAL::protobuf::CSubscriber<pb::Imu>> imu_subscribers;
    std::vector<eCAL::protobuf::CSubscriber<pb::NavSatFix>> fix_subscribers;
    std::vector<eCAL::protobuf::CSubscriber<pb::Odometry>> odom_subscribers;

    loc_pub_topics = loc_sub_topics;

    // 处理node index，订阅topic是上一个节点的发布的topic
    if (node_number.getValue() > 1) {
        for (int i = 0; i < loc_sub_topics.size(); i++) {
            loc_sub_topics[i] = loc_sub_topics[i] + "/" + std::to_string(node_number.getValue() - 1);
        }

        loc_stop_sub_topic = loc_stop_sub_topic + "/" + std::to_string(node_number.getValue() - 1);
    }

    for (int i = 0; i < loc_pub_topics.size(); i++) {
        loc_pub_topics[i] = loc_pub_topics[i] + "/" + std::to_string(node_number.getValue());
    }

    loc_stop_pub_topic = loc_stop_pub_topic + "/" + std::to_string(node_number.getValue());

    for (int i = 0; i < loc_sub_topics.size(); i++) {
        if (i >= topic_num.getValue()) {
            break;
        }

        // 判断 loc_sub_topics[i] 是否包含 fix odom imu
        if (loc_sub_topics[i].find("fix") != std::string::npos) {
            auto fix_callback = std::bind(OnNavSatFix, std::placeholders::_1, std::placeholders::_2,
                                          std::placeholders::_3, std::placeholders::_4);
            eCAL::protobuf::CSubscriber<pb::NavSatFix> fix_sub(loc_sub_topics[i]);
            fix_sub.AddReceiveCallback(fix_callback);
            fix_subscribers.push_back(std::move(fix_sub));
            std::cout << "fix subscribers [" << i + 1 << "] topic name : " << loc_sub_topics[i] << std::endl;
        } else if (loc_sub_topics[i].find("odom") != std::string::npos) {
            auto odom_callback = std::bind(OnOdometry, std::placeholders::_1, std::placeholders::_2,
                                           std::placeholders::_3, std::placeholders::_4);
            eCAL::protobuf::CSubscriber<pb::Odometry> odom_sub(loc_sub_topics[i]);
            odom_sub.AddReceiveCallback(odom_callback);
            odom_subscribers.push_back(std::move(odom_sub));
            std::cout << "odom subscribers [" << i + 1 << "] topic name : " << loc_sub_topics[i] << std::endl;
        } else if (loc_sub_topics[i].find("imu") != std::string::npos) {
            auto imu_callback = std::bind(OnImu, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
                                          std::placeholders::_4);
            eCAL::protobuf::CSubscriber<pb::Imu> imu_sub(loc_sub_topics[i]);
            imu_sub.AddReceiveCallback(imu_callback);
            imu_subscribers.push_back(std::move(imu_sub));
            std::cout << "imu subscribers [" << i + 1 << "] topic name : " << loc_sub_topics[i] << std::endl;
        }
    }

    if (topic_num.getValue() == 0) {
        loc_stop = true;
    } else {
        // subscriber stop
        eCAL::CSubscriber stop_sub(loc_stop_sub_topic);
        stop_sub.AddReceiveCallback([&](const char* topic_name_, const struct eCAL::SReceiveCallbackData* data_) {
            if ("stop" == std::string((char*)data_->buf, data_->size)) {
                std::cout << "stop signal received by " << topic_name_ << std::endl;
                loc_stop = true;
            }
        });
        std::cout << "loc stop topic: " << loc_stop_sub_topic << std::endl;
        stop_signal_subscribers.push_back(std::move(stop_sub));
    }

    if (send_flag) {
        for (int i = 0; i < topic_num.getValue(); i++) {
            if (i >= topic_num.getValue()) {
                break;
            }

            if (loc_pub_topics[i].find("fix") != std::string::npos) {
                if (fix_publishers.find(loc_sub_topics[i]) != fix_publishers.end()) {
                    continue;
                }
                auto pub = new eCAL::protobuf::CPublisher<pb::NavSatFix>(loc_pub_topics[i]);
                fix_publishers[loc_sub_topics[i]] = pub;
                std::cout << "fix publishers [" << i + 1 << "] topic name : " << loc_pub_topics[i] << std::endl;
            } else if (loc_pub_topics[i].find("odom") != std::string::npos) {
                if (odom_publishers.find(loc_sub_topics[i]) != odom_publishers.end()) {
                    continue;
                }
                auto pub = new eCAL::protobuf::CPublisher<pb::Odometry>(loc_pub_topics[i]);
                odom_publishers[loc_sub_topics[i]] = pub;
                std::cout << "odom publishers [" << i + 1 << "] topic name : " << loc_pub_topics[i] << std::endl;
            } else if (loc_pub_topics[i].find("imu") != std::string::npos) {
                if (imu_publishers.find(loc_sub_topics[i]) != imu_publishers.end()) {
                    continue;
                }
                auto pub = new eCAL::protobuf::CPublisher<pb::Imu>(loc_pub_topics[i]);
                imu_publishers[loc_sub_topics[i]] = pub;
                std::cout << "imu publishers [" << i + 1 << "] topic name : " << loc_pub_topics[i] << std::endl;
            }
        }
    }

    eCAL::CPublisher stop_signal_pub(loc_stop_pub_topic);

    // 捕获信号
    signal(SIGINT, [](int) {
        std::cout << "\nstop signal received" << std::endl;
        loc_stop = true;
    });

    while (true) {
        // sleep 500 ms
        eCAL::Process::SleepMS(500);
        if (loc_stop) {
            break;
        }
    }

    eCAL::Process::SleepMS(1000);

    for (auto eva : evaluate_map) {
        evaluate(eva.second, eva.first);
    }

    std::cout << all_info.str();

    stop_signal_pub.Send("stop");
    eCAL::Process::SleepMS(1000);

    stop_signal_pub.Send("stop");
    eCAL::Process::SleepMS(1000);

    // finalize eCAL API
    eCAL::Finalize();

    std::cout << sync_all_info.str() << std::endl;

    return (0);
}
